function trajectory = generate_true_trajectory(steps, dt, v_base, omega_base, Q, obstacles)
% 生成真实轨迹（带避障，统一用于对比）
% 输入: steps-时间步数, dt-时间步长, v_base-基础速度, omega_base-基础角速度
%       Q-过程噪声, obstacles-障碍物
% 输出: trajectory-真实轨迹3xN

    trajectory = zeros(3, steps);
    
    % 固定初始方向：水平向右
    initial_theta = 0;  % 0弧度 = 水平向右
    state = [25; 25; initial_theta];  % 中心起点，朝向右方
    
    % 边界
    boundary = struct();
    boundary.x_min = 5;
    boundary.x_max = 45;
    boundary.y_min = 5;
    boundary.y_max = 45;
    boundary.margin = 1.5;
    
    % 避障参数
    safety_margin = 1.5;
    
    boundary_avoiding = false;
    obstacle_avoiding = false;
    avoidance_timer = 0;
    angle_diff = 0;
    v = v_base;
    omega = 0;  % 初始角速度为0，直线前进
    
    for t = 1:steps
        x = state(1);
        y = state(2);
        theta = state(3);
        
        % 障碍物碰撞检测
        [collision, escape_dir] = check_collision(state, obstacles, safety_margin);
        
        if collision && ~obstacle_avoiding
            obstacle_avoiding = true;
            avoidance_timer = 30;
            target_theta = atan2(escape_dir(2), escape_dir(1));
            angle_diff = wrapToPi(target_theta - theta);
            % 限制最大转向角度为120度，避免急转弯
            if abs(angle_diff) > 2*pi/3
                angle_diff = sign(angle_diff) * 2*pi/3;
            end
        end
        
        % 边界检测
        near_x_min = (x - boundary.x_min) < boundary.margin;
        near_x_max = (boundary.x_max - x) < boundary.margin;
        near_y_min = (y - boundary.y_min) < boundary.margin;
        near_y_max = (boundary.y_max - y) < boundary.margin;
        at_boundary = near_x_min || near_x_max || near_y_min || near_y_max;
        
        if at_boundary && ~boundary_avoiding
            boundary_avoiding = true;
            avoidance_timer = 50;
            % 平滑转向：选择较小的转向角度
            target_theta = theta;
            if near_x_min
                % 向右转（0度或90度方向，选择较近的）
                opt1 = 0;
                opt2 = pi/2;
                if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                    target_theta = opt1;
                else
                    target_theta = opt2;
                end
            end
            if near_x_max
                % 向左转（180度或-90度方向，选择较近的）
                opt1 = pi;
                opt2 = -pi/2;
                if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                    target_theta = opt1;
                else
                    target_theta = opt2;
                end
            end
            if near_y_min
                % 向上转（90度或0度方向，选择较近的）
                opt1 = pi/2;
                opt2 = 0;
                if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                    target_theta = opt1;
                else
                    target_theta = opt2;
                end
            end
            if near_y_max
                % 向下转（-90度或180度方向，选择较近的）
                opt1 = -pi/2;
                opt2 = pi;
                if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                    target_theta = opt1;
                else
                    target_theta = opt2;
                end
            end
            angle_diff = wrapToPi(target_theta - theta);
            % 限制最大转向角度为120度
            if abs(angle_diff) > 2*pi/3
                angle_diff = sign(angle_diff) * 2*pi/3;
            end
            obstacle_avoiding = false;  % 边界优先
        end
        
        % 生成控制输入
        if (boundary_avoiding || obstacle_avoiding) && avoidance_timer > 0
            v = v_base * 0.5;
            % 平滑转向：根据角度差调整转速
            omega = sign(angle_diff) * min(abs(angle_diff) / dt / 10, 0.3);
            avoidance_timer = avoidance_timer - 1;
            if avoidance_timer == 0
                boundary_avoiding = false;
                obstacle_avoiding = false;
            end
        else
            % 正常探索：平滑运动，避免大幅转向
            if mod(t, 60) == 0  % 增加调整间隔（从40到60），保持方向稳定性
                rng(t);
                v = v_base * (0.9 + 0.2 * rand());  % 速度更稳定（0.9-1.1倍）
                
                % 平滑角速度调整：基于当前角速度微调，避免突变
                if t == 60
                    % 初次设置：小范围随机
                    omega = omega_base * (-0.5 + rand());  % -0.5~0.5范围
                else
                    % 后续调整：在当前值基础上微调（±30%）
                    omega_change = omega * (-0.3 + 0.6 * rand());
                    omega = omega + omega_change;
                    % 限制最大角速度，避免急转弯
                    omega = max(-0.2, min(0.2, omega));
                end
            end
        end
        
        % 运动
        control = [v; omega];
        state = motion_model(state, control, dt, Q);
        
        % 安全检查
        [still_collision, ~] = check_collision(state, obstacles, 0.5);
        if still_collision && t > 1
            state = trajectory(:, t-1);
            % 减小紧急转向角速度
            omega = 0.5 * sign(randn());
            obstacle_avoiding = true;
            avoidance_timer = 20;
        end
        
        trajectory(:, t) = state;
    end
end
